#include "camera_trigger.h"
#include "sensor_utils.hpp"

#include <fstream>

using namespace sensor::camera;
using namespace sensor::utils;

CameraTrigger::CameraTrigger(muduo::net::EventLoop* loop, double period_s)
    : loop_(loop)
    , period_s_(period_s)
    , logger_(rclcpp::get_logger("CameraTrigger"))
{
    CommandExecutor exec(rclcpp::get_logger("v4l2"));
    exec.run("sudo chmod 777 /sys/class/gpio/export");
    exec.run("sudo chmod 777 /sys/class/gpio/unexport");

    if (0 != ::access("/sys/class/gpio/PCC.00", F_OK)) {
        std::fstream fout("/sys/class/gpio/export", std::ios::out);
        fout << "328";
    }

    if (0 == ::access("/sys/class/gpio/PCC.00", F_OK)) {
        std::fstream fout("/sys/class/gpio/PCC.00/direction", std::ios::out);
        fout << "out";
    } else {
        RCLCPP_ERROR(logger_, "\"/sys/class/gpio/PCC.00\" not exist!");
    }
}

void CameraTrigger::setTriggerBeginCallback(const TriggerBeginCallback& cb)
{
    cb_ = cb;
}

void CameraTrigger::run()
{
    loop_->runEvery(period_s_, std::bind(&CameraTrigger::do_trigger, this));
}

void CameraTrigger::do_trigger()
{
    if (cb_) {
        cb_();
    }

    {
        std::fstream fout("/sys/class/gpio/PCC.00/value", std::ios::out);
        fout << "1";
    }

    usleep(12 * 1000);

    {
        std::fstream fout("/sys/class/gpio/PCC.00/value", std::ios::out);
        fout << "0";
    }
}
